/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 深圳市华友高科有限公司
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "maplogic.h"
#include <qdebug.h>
#include <fstream>
#include "yaml.h"
#include <qfile.h>
#include "../CommunicateDelegate/globaldata.h"
#include "hmicommondefine.h"
#include "instructionEnum.h"


MapLogic::MapLogic()
{
    mapResolution=0.05;
    origin_x=0;
    origin_y=0;
    origin_a=0;
    mapWidth=1600;
    mapHeight=864;

    frontLength=0;
    backLength=0;
    leftLength=0;
    rightLength=0;

//    frontLength=18;
//    backLength=18;
//    leftLength=10;
//    rightLength=10;



    //    readYamlFile("testPath");
}

int MapLogic::getOriginOffset(double &xOut, double &yOut, double &rOut)
{
    xOut=origin_x;
    yOut=origin_y;
    rOut=origin_a;
    return 1;
}

int MapLogic::setCarBorderAndOffset(std::vector<VehiclePosition> &carBorderOffsetIn)
{
    if(carBorderOffsetIn.size()>=4)
    {
        frontLength=carBorderOffsetIn[0].x/mapResolution;
        backLength=-carBorderOffsetIn[2].x/mapResolution;
        leftLength=carBorderOffsetIn[0].y/mapResolution;
        rightLength=-carBorderOffsetIn[1].y/mapResolution;
    }
<<<<<<< HEAD

=======
>>>>>>> dev
}



QString MapLogic::getMapPgmFilePath()
{
    return imageFilePath;
}

int MapLogic::reloadMap(MapPropertyInfo infoIn)
{
    mapResolution=infoIn.mapRatio;
    origin_x=infoIn.xOffset;
    origin_y=infoIn.yOffset;
    origin_a=infoIn.rOffset;
    imageFilePath=D_MAP_PATH;
    imageFilePath+=QString::fromStdString(infoIn.imageName);
    mapWidth=infoIn.imageWidth;
    mapHeight=infoIn.imageHeight;

//    if(0)
//    {
//        //文件读取
//        QString tmpImageName;
//        if(1!=readYamlFile(mapYamlPath,mapResolution,origin_x,origin_y,origin_a,tmpImageName))
//        {
//            return -1;
//        }
//        else
//        {
//                imageFilePath=D_MAP_PATH;
//                imageFilePath+=tmpImageName;
//                if(1!=getMapFileSize(imageFilePath,mapWidth,mapHeight))
//                {
//                    return -1;
//                }
//        }
//    }
//    else
//    {


//    }

    return 1;
}
//void MapLogic::calculateCarPoint(double x, double y, double a,QVector<double>& xlist,QVector<double>& ylist)
//{
//    xlist.resize(4);
//    ylist.resize(4);
//    xlist[0]=x-leftLength;
//    xlist[1]=x+rightLength;
//    xlist[2]=x-leftLength;
//    xlist[3]=x+rightLength;

//    ylist[0]=y-frontLength;
//    ylist[1]=y-frontLength;
//    ylist[2]=y+backLength;
//    ylist[3]=y+backLength;
//    return ;

//}

void MapLogic::calculateCarTransMatrix(double car_x_pix, double car_y_pix, double car_angle_degree,
                                 QMatrix& tmpMatrixOut)
{
    tmpMatrixOut.rotate(car_angle_degree);

    QPoint tmpPoint1(leftLength,frontLength);
    QMatrix tmpMatrix;
    tmpMatrix.rotate(car_angle_degree);
    QPoint tmpPoint2=tmpMatrix.map(tmpPoint1);
    double tmpx=car_x_pix-tmpPoint2.x();
    double tmpy=car_y_pix-tmpPoint2.y();
    tmpMatrixOut.translate(tmpx,tmpy);
    QMatrix tmpMatrixReturn(tmpMatrix.m11(),tmpMatrix.m12(),tmpMatrix.m21(),tmpMatrix.m22(),tmpx,tmpy);
    tmpMatrixOut=tmpMatrixReturn;
//    qDebug()<<tmpMatrix;
    //    qDebug()<<tmpMatrixOut<<"matrix2";
}

void MapLogic::calculateDPointMatrix(double pictureWidth,double car_x_pix, double car_y_pix, double car_angle_degree, QMatrix &tmpMatrixOut)
{
    tmpMatrixOut.rotate(car_angle_degree);

    QPoint tmpPoint1(pictureWidth/2,pictureWidth/2);
    QMatrix tmpMatrix;
    tmpMatrix.rotate(car_angle_degree);
    QPoint tmpPoint2=tmpMatrix.map(tmpPoint1);
    double tmpx=car_x_pix-tmpPoint2.x();
    double tmpy=car_y_pix-tmpPoint2.y();
    tmpMatrixOut.translate(tmpx,tmpy);
    QMatrix tmpMatrixReturn(tmpMatrix.m11(),tmpMatrix.m12(),tmpMatrix.m21(),tmpMatrix.m22(),tmpx,tmpy);
    tmpMatrixOut=tmpMatrixReturn;
//    qDebug()<<tmpMatrix;
    //    qDebug()<<tmpMatrixOut<<"matrix2";
}

int MapLogic::calculateLaserTransMatrix(double car_x_pix, double car_y_pix, double car_angle_degree, QMatrix &tmpMatrixOut)
{
    QMatrix tmpMatrixReturn;
    tmpMatrixReturn.translate(car_x_pix,car_y_pix);
    tmpMatrixReturn.rotate(car_angle_degree);
    tmpMatrixOut=tmpMatrixReturn;
    return 1;
}

int MapLogic::setCarProfile(double frontLengthIn, double backLengthIn,
                           double leftLengthIn, double rightLengthIn)
{
    frontLength=frontLengthIn;
    backLength=backLengthIn;
    leftLength=leftLengthIn;
    rightLength=rightLengthIn;
    return 1;
}

double MapLogic::getMapWidth()
{
    return mapWidth;
}

double MapLogic::getMapHeight()
{
    return mapHeight;
}

double MapLogic::getCarWidth()
{
    return leftLength+rightLength;
}

double MapLogic::getCarHeight()
{
    return frontLength+backLength;
}

double MapLogic::getMapResolution()
{
    return mapResolution;
}

int MapLogic::translateScenePoint2WorldPoint(double x_in, double y_in, double a_in, double &x_out, double &y_out, double &a_out)
{
    x_out=x_in*mapResolution+origin_x;
    y_out=(mapHeight-y_in)*mapResolution+origin_y;
    a_out=-a_in;
    return 1;
}

int MapLogic::translateWorldPoint2ScenePoint(double x_in, double y_in, double a_in, double &x_out, double &y_out, double &a_out)
{
    x_out=(x_in-origin_x)/mapResolution;
    y_out=mapHeight-(y_in-origin_y)/mapResolution;
    a_out=-a_in+90.0;
    return 1;
}

double MapLogic::translateToSceneLength(double worldLength)
{
    return worldLength/mapResolution;
}

//int MapLogic::translateWorldPoint2ScenePoint_noBias(double x_in, double y_in, double a_in,
//                                                    double &x_out, double &y_out, double &a_out)
//{
//    x_out=(x_in)/mapResolution;
//    y_out=-y_in/mapResolution;
//    a_out=-a_in;
//    return 1;
//}

struct Vec3 {
   float x, y, a;
};

void operator >> (const YAML::Node& node, Vec3& v) {
   node[0] >> v.x;
   node[1] >> v.y;
   node[2] >> v.a;
}

//void operator >> (const YAML::Node& node, Monster& monster) {
//   node["name"] >> monster.name;
//   node["position"] >> monster.position;
//   const YAML::Node& powers = node["powers"];
//   for(unsigned i=0;i<powers.size();i++) {
//      Power power;
//      powers[i] >> power;
//      monster.powers.push_back(power);
//   }
//}

int MapLogic::readYamlFile(QString yamlFile)
{
    std::ifstream fin(yamlFile.toLatin1());
//    if(0==fin.gcount())
//    {
//        return -1;
//    }
    YAML::Parser parser(fin);

    YAML::Node doc;
    if(false==parser.GetNextDocument(doc))
    {
       qDebug()<<"error, no file found";
       return -1;
    }

//    QString pattern = "V(\\d+).(\\d+)\\s*(.*)";
//    QRegExp rx(pattern);
//    bool isMatch=rx.exactMatch(tmpVersion);
//    mainVersion=rx.cap(1).toInt();
//    subVersion=rx.cap(2).toInt();
    imageFilePath.clear();
    imageFilePath=D_MAP_PATH;
    std::string  tmpString;
    doc["image"] >> tmpString;
    //cartographer生成的是图片的绝对路径
    QString tmpFileName=QString::fromStdString(tmpString);
    if(tmpFileName.contains('/'))
    {
        int first = tmpFileName.lastIndexOf ("/"); //从后面查找"/"位置
        QString title = tmpFileName.right (tmpFileName.length ()-first-1); //从右边截取
        imageFilePath+=title;
    }
    else
    {
        imageFilePath+=QString::fromStdString(tmpString);
    }

    qDebug()<<imageFilePath;

    doc["resolution"] >> mapResolution;

    Vec3 originVec;
    doc["origin"] >> originVec;
    origin_x=originVec.x;
    origin_y=originVec.y;
    origin_a=originVec.a;

    return 1;

}

int MapLogic::readYamlFile(QString yamlFilePath,double &mapResolutionOut,
                         double &xOffsetOut,double &yOffsetOut,double &rOffsetOut,QString &imageNameOut)
{
    std::ifstream fin(yamlFilePath.toLatin1());
//    if(0==fin.gcount())
//    {
//        return -1;
//    }

    Vec3 originVec;
    try
    {
        YAML::Parser parser(fin);

        YAML::Node doc;
        if(false==parser.GetNextDocument(doc))
        {
           qDebug()<<"error, no file found,readYamlFile"<<yamlFilePath;
           return -1;
        }
        std::string  tmpString;
        doc["image"] >> tmpString;
        //cartographer生成的是图片的绝对路径
        QString tmpFileName=QString::fromStdString(tmpString);
        if(tmpFileName.contains('/'))
        {
            int first = tmpFileName.lastIndexOf ("/"); //从后面查找"/"位置
            imageNameOut = tmpFileName.right (tmpFileName.length ()-first-1); //从右边截取
        }
        else
        {
            imageNameOut=QString::fromStdString(tmpString);
        }

        doc["resolution"] >> mapResolutionOut;


        doc["origin"] >> originVec;
    }
    catch(YAML::ParserException  errorInfo)
    {
        qDebug()<<"error, yaml格式错误"<<yamlFilePath;
        return -2;
    }

    xOffsetOut=originVec.x;
    yOffsetOut=originVec.y;
    rOffsetOut=originVec.a;

    return 1;

}


int MapLogic::getMapFileSize(QString pgmFile, double &width, double &heighth)
{
    QString displayString;
    QFile file(pgmFile);
     if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
         return -1;

     QTextStream in(&file);
     QString line = in.readLine();
     QString lineSecond = in.readLine();//gmapping 创建的本行是＃开头注释，ｐｓ创建的本行是宽
     QString lineThird = in.readLine();//gmapping 创建的本行是宽和高，ｐｓ创建的本行是高
     if(lineSecond.contains('#'))
     {
         qDebug()<<"gmapping picture";


         if (true!=line.isNull())
         {
             QString firstStr=lineThird.section(" ",0,0);
             qDebug()<<firstStr;
             QString secondStr=lineThird.section(" ",1,1);
             qDebug()<<secondStr;
             width=firstStr.toInt();
             heighth=secondStr.toInt();
             return 1;

         }
         else
         {
             return -1;
         }
     }
     else
     {
         qDebug()<<"photoshop picture";
         width=lineSecond.toInt();
         heighth=lineThird.toInt();
     }


     return 1;


}
